#include "AxLaserCaputure.h"



AxLaserCaputure::AxLaserCaputure(ArRobot *robot, const char *name, char key, char key2)
	:ArMode(robot,name,key,key2)
	,myCaptureCB(this,&AxLaserCaputure::CapturePointCloud)
	,isStarCapture(false)
{
}


AxLaserCaputure::~AxLaserCaputure(void)
{
}

void AxLaserCaputure::activate(void)
{
	if (!baseActivate())
		return;
	if (myRobot == NULL)
		return;
	addKeyHandler('1', &myCaptureCB);
	//addKeyHandler('X', &myCaptureCB);
}

void AxLaserCaputure::deactivate(void)
{
	if (!baseDeactivate())
		return;

	remKeyHandler(&myCaptureCB);
	isStarCapture=false;
	ControlStartCapture=false;
	timeTickCount=0;
}
void AxLaserCaputure::userTask(void)
{
	if (myRobot == NULL)
		return;
	
	if (timeTickCount==0)
	{
		ArTime now;
		myLastDataTime.setToNow();
	}
	else
	{
		if (isStarCapture )
		{
			if(myLastDataTime.mSecSince()<30000)
			{
				printf("Laser capture is running(time=%lu), wait!\n",myLastDataTime.mSecSince());
			}
			else
			{
				printf("Reached maxTime %lu, Laser capture should be finished!\n",30000);
			}
		}
	}
	timeTickCount++;
	
}
void AxLaserCaputure::CapturePointCloud(void)
{
	isStarCapture=true;
	ControlStartCapture=true;
}

void AxLaserCaputure::help(void)
{
	ArLog::log(ArLog::Terse, "Capture mode will let you control the 3D laser scanner.");
}
